//
// shit code！！！！！ 这个类应该继承offboardManager 现在的这种设计模式导致中间出现了大量指针调用函数，且部分函数设计十分麻烦
// fuck 先凑活用吧，fkfkfkfkfk,现在这个是针对当前集群信息的管理，也有道理
// Created by sundx on 2020/11/16.
//
#include "SwarmCore.h"


SwarmCore::SwarmCore() {
    om = new OffboardManager(nh);
    updateParams();
    swarmPosition.header.stamp = ros::Time::now();
    swarmPosition.swarm_pose = vector<geometry_msgs::Point>(swarm_max_num);
    swarmPosition.swarm_vel = vector<geometry_msgs::Point>(swarm_max_num);
    swarmReadyStatus = vector<uint8_t>(swarm_max_num);
    om->state.cur = first_takeoff;
    om->state.current_run_finished = false;
    om->state.next = second_wait_swarm;
    initSubAndPub();
}

void
SwarmCore::initSubAndPub() {
    uwb_data_sub = nh.subscribe("/incoming_broadcast_data", 1, &SwarmCore::parseTopicData, this);
    sp_pos_sub = nh.subscribe("/global_drone_target_position_id" + to_string(om->id), 1,
                              &SwarmCore::setSpPose, this);
    // uwb_data_sub = nh.subscribe("/send_topic", 1, &SwarmCore::parseTopicData,this);
    if (om->using_gazebo) {
        position_pub = nh.advertise<incoming_broadcast_data>("/incoming_broadcast_data", 1);
    } else {
        position_pub = nh.advertise<data_buffer>("/send_topic", 1);
    }
    world_position_sub = nh.subscribe("/camera_world_position_id"+to_string(om->id),1,&SwarmCore::worldPositionUpdate,this);
    swarm_state_sub = nh.subscribe("/swarm_state/remote_nodes",1,&SwarmCore::swarmStateUpdate,this);
    
    swarm_position_pub = nh.advertise<Swarm_Position>("/swarm_position_id" + to_string(om->id), 1);

    ConstructSendFrameAndPub_timer = nh.createTimer(ros::Duration(0.033), &SwarmCore::ConstructSendFrameAndPub, this);
    swarmPositionPub_timer = nh.createTimer(ros::Duration(0.02), &SwarmCore::swarmPositionPub, this);
}

void 
SwarmCore::swarmStateUpdate(const remote_uwb_info &msg){
    swarm_state.updateStateFromUWB(msg);
}

void 
SwarmCore::worldPositionUpdate(const geometry_msgs::PoseStamped &msg){
    camera_global_pos[0] = msg.pose.position.x;
    camera_global_pos[1] = msg.pose.position.y;
    camera_global_pos[2] = msg.pose.position.z;
    tf::Quaternion camera_att;
    tf::quaternionMsgToTF(msg.pose.orientation, camera_att);
    double roll = 0;    double pitch = 0;    double yaw = 0;
    tf::Matrix3x3(camera_att).getRPY(roll, pitch, yaw);
    camera_global_yaw = (float)yaw;
}

void
SwarmCore::swarmPositionPub(const ros::TimerEvent &e) {
    swarmPosition.header.stamp = ros::Time::now();
    if (om->id <= 4) {
        swarmPosition.swarm_pose[om->id].x = global_position[0];
        swarmPosition.swarm_pose[om->id].y = global_position[1];
        swarmPosition.swarm_pose[om->id].z = global_position[2];
        swarmPosition.swarm_vel[om->id].x = om->cur_vel[0];
        swarmPosition.swarm_vel[om->id].y = om->cur_vel[1];
        swarmPosition.swarm_vel[om->id].z = om->cur_vel[2];
        swarmReadyStatus[om->id] = om->take_off_finished;
    }
    SwarmStates::UWBSwarmState cur_swarm_state = swarm_state.currentState;
    if (om->using_gazebo||cur_swarm_state.swarmActive.size()<1) {
        if (om->take_off_finished) {
            for (int j = 0; j < swarmReadyStatus.size(); ++j) {
                if (swarmPosition.swarm_pose[j].z < om->takeoff_height / 10) {
                    swarmReadyStatus[j] = 1;
                }
            }
        }
    }
    swarm_position_pub.publish(swarmPosition);
}

void
SwarmCore::updateValues() {
    float gox = (om->init_pose[0]) +
                (-init_global_position[1] * sin(init_global_yaw - om->init_yaw) -
                 init_global_position[0] * cos(init_global_yaw - om->init_yaw));
    float goy = (om->init_pose[1]) +
                (-init_global_position[1] * cos(init_global_yaw - om->init_yaw) +
                 init_global_position[0] * sin(init_global_yaw - om->init_yaw));
    gox = om->cur_pos[0] - gox;
    goy = om->cur_pos[1] - goy;
    float gyaw = om->init_yaw - init_global_yaw;
    global_position[0] = goy * sin(gyaw) + gox * cos(gyaw);
    global_position[1] = goy * cos(gyaw) - gox * sin(gyaw);
    global_position[2] = init_global_position[2] + (om->cur_pos[2] - om->init_pose[2]);
}

void
SwarmCore::ConstructSendFrameAndPub(const ros::TimerEvent &e) {
    Buffer send_buffer;
    send_frame.id = om->id;
    send_frame.takeoff_finished = om->take_off_finished;
    send_frame.global_position_x = (int) (SCALE * global_position[0]);
    send_frame.global_position_y = (int) (SCALE * global_position[1]);
    send_frame.global_position_z = (int) (SCALE * global_position[2]);
    send_frame.vx = (int) (SCALE * om->cur_vel[0]);
    send_frame.vy = (int) (SCALE * om->cur_vel[1]);
    send_frame.vz = (int) (SCALE * om->cur_vel[2]);
    Buffer data;
    data.resize(sizeof(send_frame));
    memcpy(&data[0], &send_frame, sizeof(send_frame));
    send_buffer.push_back(0xFE); //head
    send_buffer.push_back(0x22); //head
    unsigned char chk = 0x00;
    for (unsigned char d : data) {
        chk += d;
    }
    send_buffer.insert(send_buffer.end(), data.begin(), data.end());
    send_buffer.push_back(chk); //chk
    send_buffer.push_back(0xEE); //tail
    if (om->using_gazebo) {
        incoming_broadcast_data send_msg;
        send_msg.data = send_buffer;
        send_msg.source = 2;
        send_msg.header.stamp = ros::Time::now();
        position_pub.publish(send_msg);
    } else {
        data_buffer send_msg;
        send_msg.data = send_buffer;
        send_msg.send_method = 2;
        send_msg.header.stamp = ros::Time::now();
        position_pub.publish(send_msg);
    }
}

void
SwarmCore::parseTopicData(const incoming_broadcast_data &msg) {
    swarmPosition.header = msg.header;
    Buffer recv_buffer;
    recv_buffer = msg.data;
    if (recv_buffer.size() != sizeof(send_frame) + 4)return;
    if (recv_buffer[0] == 0xFE) {
        if (recv_buffer[1] == 0x22) {
            Buffer data;
            unsigned char chk = 0x00;
            data.resize(sizeof(send_frame));
            for (int i = 0; i < data.size(); ++i) {
                data[i] = recv_buffer[i + 2];
                chk += data[i];
            }
            if (recv_buffer[sizeof(send_frame) + 2] == chk) {
                POSITION_FRAME recv_frame{};
                memcpy(&recv_frame, &data[0], sizeof(recv_frame));
                if (recv_frame.id == om->id)return;
                swarmPosition.swarm_pose[recv_frame.id].x = recv_frame.global_position_x / SCALE;
                swarmPosition.swarm_pose[recv_frame.id].y = recv_frame.global_position_y / SCALE;
                swarmPosition.swarm_pose[recv_frame.id].z = recv_frame.global_position_z / SCALE;
                swarmPosition.swarm_vel[recv_frame.id].x = recv_frame.vx / SCALE;
                swarmPosition.swarm_vel[recv_frame.id].y = recv_frame.vy / SCALE;
                swarmPosition.swarm_vel[recv_frame.id].z = recv_frame.vz / SCALE;
                swarmReadyStatus[recv_frame.id] = recv_frame.takeoff_finished||swarmReadyStatus[recv_frame.id];
            }
        }
    }
}

void
SwarmCore::takeoff() {
    om->try_switch_offboard_armed();
}

bool
SwarmCore::swarmReady() {
    bool res = true;
    SwarmStates::UWBSwarmState cur_swarm_state = swarm_state.currentState;
    if (om->using_gazebo) {
        for (int j = 0; j < swarm_max_num - 1; ++j) {
            res = swarmReadyStatus[j];
            if (!res)return res;
        }
        return res;
    } else {
        if (cur_swarm_state.swarmActive.size() > 1) {
            for (int j = 0; j < cur_swarm_state.swarmIds.size(); ++j) {
                int remote_id = cur_swarm_state.swarmIds[j];
                if (cur_swarm_state.swarmActive[j]) {
                    res = swarmReadyStatus[remote_id];
                    if (!res)return res;
                }
            }
        } else {
            //此时只有一架无人机，即自身，也可以执行 一般此场景用于测试，本质应为不连接任何节点的情况，保持一动不动
            res = true;
        }
        return res;
    }
}

vector<float>
SwarmCore::getSwarmDistance(){
    vector<float> res(swarmPosition.swarm_pose.size());
    vector<float> cur_pos(3);
    cur_pos[0] = swarmPosition.swarm_pose[om->id].x;
    cur_pos[1] = swarmPosition.swarm_pose[om->id].y;
    cur_pos[2] = swarmPosition.swarm_pose[om->id].z;
    for (int j = 0; j <res.size(); ++j) {
        vector<float> to_pos(3);
        to_pos[0] = swarmPosition.swarm_pose[j].x;
        to_pos[1] = swarmPosition.swarm_pose[j].y;
        to_pos[2] = swarmPosition.swarm_pose[j].z;
        res[j] = RosMath::getDistance(cur_pos,to_pos);
    }
}

void
SwarmCore::mainLoop() {
    ros::Rate loop_rate(om->rate);
    ros::AsyncSpinner spinner(3);
    spinner.start();
    while (ros::ok()) {
        ros::spinOnce();
        updateValues();
        if (om->state.flyMode != offboard) {
            om->holdZeroPosition();
            om->init_position(loop_rate, 1);
            ros::spinOnce();
            if (!om->using_gazebo)continue;
        }
        if (om->state.armState != armed) {
            om->holdZeroPosition();
            om->init_position(loop_rate, 1);
            ros::spinOnce();
            if (!om->using_gazebo)continue;
        }

        if (om->state.current_run_finished) {
            ROS_INFO_STREAM("current_run state " << om->stateString(om->state.cur)
                                                 << " finished ,run next one: "
                                                 << om->stateString(om->state.next));
            om->state.cur = om->state.next;
            om->state.current_run_finished = false;
        }

        switch (om->state.cur) {
            case first_takeoff:
                om->state.next = second_wait_swarm;
                if (om->state.flyMode != offboard && om->state.armState != armed) {
                    om->try_switch_offboard_armed();
                }
                om->takeoff(om->takeoff_height);
                om->state.current_run_finished = om->take_off_finished;
                break;
            case second_wait_swarm:
                om->state.next = third_start_task;
                om->holdInitPos(om->takeoff_height);
                om->state.current_run_finished = swarmReady();
                break;
            case third_start_task:
                om->state.next = fourth_land;
                if ((fabs(sp_pos_v[0]) + fabs(sp_pos_v[1])) < 0.0001) {
                    om->holdInitPos(om->takeoff_height);
                    ROS_INFO_STREAM_THROTTLE(0.2,
                                             "["<<om->id<<"]send sp pos error: sp_pos_Vx: " << sp_pos_v[0] << " sp_pos_Vy:" << sp_pos_v[1]);
                } else {
                    // offboardManager->moveByV(sp_pos_v[0]*v_scale, sp_pos_v[1]*v_scale, 0);
                    // avoidCrash();
                    float vx_scale = 1;
                    float vy_scale = 1;
                    if(fabs(sp_pos_v[0])<1){
                        vx_scale = abs(sp_pos_v[0]);
                    }
                    if(fabs(sp_pos_v[1])<1){
                        vy_scale = abs(sp_pos_v[1]);
                    }
                    om->moveto(
                            sp_pos_v[0]*v_scale*vx_scale,
                            sp_pos_v[1]*v_scale*vy_scale,
                            om->takeoff_height,
                            om->cur_pos[0],
                            om->cur_pos[1],
                            om->init_pose[2]);
                }
                break;
            case fourth_land:
            default:
                break;
        }

        loop_rate.sleep();
    }
    ros::waitForShutdown();
}

void
SwarmCore::testLoop(){
    ros::Rate loop_rate(om->rate);
    ros::AsyncSpinner spinner(3);
    spinner.start();
    run.current_run_finished = false;
    run.cur_run = first_takeoff_test;
    while (ros::ok()){
        ros::spinOnce();
        updateValues();
        if (om->state.flyMode != offboard) {
            om->holdZeroPosition();
            om->init_position(loop_rate, 1);
            ros::spinOnce();
            if (!om->using_gazebo)continue;
        }
        if (om->state.armState != armed) {
            om->holdZeroPosition();
            om->init_position(loop_rate, 1);
            ros::spinOnce();
            if (!om->using_gazebo)continue;
        }
        if(run.current_run_finished){
            run.cur_run = run.next_run;
            run.current_run_finished = false;
            ROS_INFO_STREAM("current run finished run next :"<< run.next_run);
        }
        switch (run.cur_run){
        case first_takeoff_test:
            run.next_run = sencond_rotate_yaw;
            if (om->state.flyMode != offboard && om->state.armState != armed) {
                om->try_switch_offboard_armed();
            }
            om->takeoff(om->takeoff_height);
            run.current_run_finished = om->take_off_finished;
            if(run.current_run_finished){
                om->record_pose();
            }
            break;
        case sencond_rotate_yaw:
            run.next_run = third_go_x;
            om->rotateYawCCW(PI/2,
            om->last_recorded_pose.yaw,
            om->last_recorded_pose.x,
            om->last_recorded_pose.y,
            om->last_recorded_pose.z);
            if(abs((om->last_recorded_pose.yaw+PI/2-om->cur_angle[2]))<0.1){
                run.current_run_finished = true;
                om->record_pose();
            }else{
                run.current_run_finished = false;
            }
            break;
        case third_go_x:
            run.next_run = fourth_go_y;
            om->moveto(1,0,0,
            om->last_recorded_pose.x,
            om->last_recorded_pose.y,
            om->last_recorded_pose.z);
            if(om->getDistanceFrom(
                om->last_recorded_pose.x+1,
                om->last_recorded_pose.y,
                om->last_recorded_pose.z)<0.1){
                run.current_run_finished = true;
                om->record_pose();
            }else{
                run.current_run_finished = false;       
            }
            break;
        case fourth_go_y:
            run.next_run = fifth_land;
            om->moveto(0,1,0,
            om->last_recorded_pose.x,
            om->last_recorded_pose.y,
            om->last_recorded_pose.z);
            if(om->getDistanceFrom(
                om->last_recorded_pose.x,
                om->last_recorded_pose.y+1,
                om->last_recorded_pose.z)<0.01){
                run.current_run_finished = true;
                om->record_pose();
            }
            break;
        case fifth_land:
            run.next_run = fifth_land;
            om->land();
            run.current_run_finished = om->land_finished;
            if(om->land_finished){
                om->setDisarmed();
                om->setPosctl();
            }
            break;
        default:
            break;
        }
    }
    ros::waitForShutdown();
}

void
SwarmCore::updateParams() {
    v_scale = RosUtils::readParam<float >(nh,"v_scale",0.5);
    safe_r = RosUtils::readParam<float >(nh,"safe_r",0.5);
    vector<float> swarm_init_position_x = RosUtils::readParam<vector<float>>(nh, "global_position_x", vector<float>(5));
    vector<float> swarm_init_position_y = RosUtils::readParam<vector<float>>(nh, "global_position_y", vector<float>(5));
    vector<float> swarm_init_position_z = RosUtils::readParam<vector<float>>(nh, "global_position_z", vector<float>(5));
    init_global_position[0] = swarm_init_position_x[om->id];
    init_global_position[1] = swarm_init_position_y[om->id];
    init_global_position[2] = swarm_init_position_z[om->id];
}

SwarmCore::~SwarmCore() {
    delete om;
}

void SwarmCore::setSpPose(const sp_target_position &msg) {
    float gox = (om->init_pose[0]) +
                (-init_global_position[1] * sin(init_global_yaw - om->init_yaw) -
                 init_global_position[0] * cos(init_global_yaw - om->init_yaw));
    float goy = (om->init_pose[1]) +
                (-init_global_position[1] * cos(init_global_yaw - om->init_yaw) +
                 init_global_position[0] * sin(init_global_yaw - om->init_yaw));
    float gx = msg.global_sp_pose.x;
    float gy = msg.global_sp_pose.y;
    float gvx = msg.global_sp_pose_v.x;
    float gvy = msg.global_sp_pose_v.y;
    float gyaw = init_global_yaw - om->init_yaw;
    sp_pos[0] = gy * sin(gyaw) + gx * cos(gyaw) + gox;
    sp_pos[1] = gy * cos(gyaw) - gx * sin(gyaw) + goy;
    sp_pos_v[0] = gvy * sin(gyaw) + gvx * cos(gyaw);
    sp_pos_v[1] = gvy * cos(gyaw) - gvx * sin(gyaw);
    ROS_INFO_STREAM_THROTTLE(0.3, "["<<om->id<<"]recv send_pos gvx:" << sp_pos_v[0] << " gvy:" << sp_pos_v[1]);
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "swarm_core");
    SwarmCore swarmCore;
    swarmCore.mainLoop();
    // swarmCore.testLoop();
}

